#include "Graph.h"

#include "stdafx.h"
#include "470bot.h"
#include "OccupancyProbabilityGrid.h"

Graph::Graph(int granularity, bool obstaclesGiven)
{
	w = World::instance();
	this->granularity = granularity;
	initialize(-w->getSize()/2, -w->getSize()/2, w->getSize()/granularity, w->getSize()/granularity);
	if(obstaclesGiven){
		buildBoundingBoxes();
		addObstacles();
	}
}


Graph::~Graph(void)
{	
}

bool Graph::initialize(int x, int y, int w, int h){
	width = w, height = h;
	this->x = x, this->y = y;
	graph.resize(w*h);

	Vector2 center(x+((float)granularity/2.0f), y + ((float)granularity/2.0f));
	float step = granularity;
	node* n;

	for(int i = 0; i < height; i ++){
		for(int j = 0; j < width; j ++){
			n = &graph[i*width+j];
			n->nextToOccupied = false;
			n->x = center.x;
			n->y = center.y;
			n->isGoal = false;
			n->previous = NULL;
			n->flag = -1;
			n->cost = -1;
			n->occupied = false;
			n->explored = false;

			center.x += step;
		}
		center.x = x+((float)granularity/2.0f);
		center.y += step;
	}

	return true;
}

bool Graph::setColumn(int col, std::string& line){
	if(line.size() < height) return false;
	
	node* n;

	for(int i = 0; i < height; i++){
		n = &graph[i*width+col];
		n->nextToOccupied = false;
		n->x = (float)(col + x) + .5f;
		n->y = (float)(i + y) + .5f;
		n->isGoal = false;
		n->previous = NULL;
		n->flag = 0;
		n->cost = -1;
		n->timesSeen = 0;

		if(line.at(i) == '1') n->occupied = true;
		else n->occupied = false;
	}
}

void Graph::buildEdges(){
	double sqrt2 = std::sqrt(2.0*std::pow((float)granularity,2));
	int nbrs[8][2];
	node* n;
	node* g;

	for(int i = 0; i < height; i++){
		for(int j = 0; j < width; j ++){
			n = &graph[i*width+j];
			nbrs[0][0] = nbrs[1][0] = nbrs[2][0] = nbrs[3][0] = nbrs[4][0] = nbrs[5][0] = nbrs[6][0] = nbrs[7][0] =-1;
			nbrs[0][1] = nbrs[1][1] = nbrs[2][1] = nbrs[3][1] = nbrs[4][0] = nbrs[5][0] = nbrs[6][0] = nbrs[7][0] =-1;

			if(j > 0){
				nbrs[0][0] = j-1;
				nbrs[0][1] = i;
			}
			if(j < width-1){
				nbrs[1][0] = j+1;
				nbrs[1][1] = i;
			}
			if(i > 0){
				if(j != 0){
					nbrs[4][0] = j-1;
					nbrs[4][1] = i-1;
				}

				nbrs[2][0] = j;
				nbrs[2][1] = i-1;
				
				if(j < width-1){
					nbrs[5][0] = j+1;
					nbrs[5][1] = i-1;
				}
			}
			if(i != height-1){
				if(j != 0){
					nbrs[6][0] = j-1;
					nbrs[6][1] = i+1;
				}
				
				nbrs[3][0] = j;
				nbrs[3][1] = i+1;
				
				if(j < width-1){
					nbrs[7][0] = j+1;
					nbrs[7][1] = i+1;
				}
			}

			for(int k = 0; k < 8; k ++){
				if(nbrs[k][0] == -1) continue;
				g = &graph[nbrs[k][1]*width+nbrs[k][0]];

				if(g->occupied){
					n->nextToOccupied = true;
					//continue;
				}
					
				if(std::abs(nbrs[k][0]-j) == 1 && std::abs(nbrs[k][1]-i) == 1){
					n->neighbors.push_back(edge::newEdge(n, g, sqrt2));
				}
				else{
					n->neighbors.push_back(edge::newEdge(n, g, granularity));
				}
			}
		}
	}
}



node* Graph::getNode(Vector2 v){
	int ty = (int)(v.y-y)/granularity;
	int tx = (int)(v.x-x)/granularity;

	return &graph[(int)(v.y-y)/granularity*width+(int)(v.x-x)/granularity];
}

Vector2 Graph::getTopLeft(){
	return Vector2(x, y);
}


void Graph::setObstacle(Vector2 v){
	int ty = (int)(v.y-y)/granularity;
	int tx = (int)(v.x-x)/granularity;
	graph[(int)(v.y-y)/granularity*width+(int)(v.x-x)/granularity].occupied = true;
}

void Graph::unsetObstacle(Vector2 v){
	int ty = (int)(v.y-y)/granularity;
	int tx = (int)(v.x-x)/granularity;
	graph[(int)(v.y-y)/granularity*width+(int)(v.x-x)/granularity].occupied = false;
}

void Graph::setGoal(Vector2 v){
	graph[(int)(v.y-y)/granularity*width+(int)(v.x-x)/granularity].isGoal = true;
}

void Graph::clearFlags(){
	node* n = NULL;
	for(int i = 0; i < height; i ++){
		for(int j = 0; j < width; j ++){
			n = &graph[i*width+j];
			n->isGoal = false;
			n->previous = NULL;
			n->flag = -1;
			n->cost = -1;
			n->nextToOccupied = false;
			n->occupied = false;
			n->explored = false;
		}
	}
}

void Graph::clearSearchFlags(){
	node* n = NULL;
	for(int i = 0; i < height; i ++){
		for(int j = 0; j < width; j ++){
			n = &graph[i*width+j];
			n->isGoal = false;
			n->previous = NULL;
			n->flag = -1;
			n->cost = -1;
		}
	}
}

void Graph::updateFromProbabilityGrid(occgrid_t* occgrid){
	OccupancyProbabilityGrid* grid = World::instance()->occupancy_probability_grid();
	
	node* n = NULL;

	int startRow = (occgrid->topCorner[1]-y)/granularity,
		startCol = (occgrid->topCorner[0]-x)/granularity,
		endRow = min(startRow + occgrid->dimensions[1]/granularity, height-1),
		endCol = min(startCol + occgrid->dimensions[0]/granularity, width-1);
	bool obstacle;


	for(int i = startRow; i <= endRow; i ++){
		for(int j = startCol; j <= endCol; j ++){
			obstacle = false;
			n = &graph[i*width+j];
			
			//float r = grid->at(n->x-x, n->y-y);
			//cout << r << endl;
			//cout << n->x << " " << n->y << " " << n->x - x << " " << n->y << " " << grid->at( n->x-x, n->y-y) << endl;
			if(grid->at(n->x-x, n->y-y) >= OBSTACLE_THRESHOLD)
				obstacle = true;

			n->explored = true;
			n->timesSeen ++;
			if(obstacle){
				n->occupied = true;
				for(int ii = 0; ii < n->neighbors.size(); ii ++){
					n->neighbors[ii].to->nextToOccupied = true;
				}
			}
			else{
				if(n->occupied){
					n->occupied = false;
					for(int ii = 0; ii < n->neighbors.size(); ii ++){
						node * next = n->neighbors[ii].to;
						next->nextToOccupied = false;
						for(int jj = 0; jj < next->neighbors.size(); jj ++){
							if(next->neighbors[jj].to->occupied){
								next->nextToOccupied = true;
								break;
							}
						}
					}
				}
			}
		}
	}
}

void Graph::SearchForUnexplored(std::vector<Vector2>* nodeCenters){
	node* n = NULL;
	bool add = true;
	float tmp = 0;

	OccupancyProbabilityGrid* opg = World::instance()->occupancy_probability_grid();

	nodeCenters->clear();
	for(int i = 1; i < height-1; i ++){
		for(int j = 1; j < width-1; j ++){
			n = &graph[i * width + j];

			/*bool undiscoverd = false;
			if(n->explored){
				for(int ii = 0; ii < granularity; ii ++){
					for(int jj = 0; jj < granularity; jj ++){
						tmp = opg->at(j*granularity+jj,i*granularity+ii);
						if(tmp >= .15 && tmp <= .9){
							n->explored = false;
							break;
						}
					}
					if(!n->explored)
						break;
				}
			}*/

			if(!n->explored){
				add = false;
				for(int ii = 0; ii < n->neighbors.size(); ii ++){
					if(n->neighbors[ii].to->explored && !n->neighbors[ii].to->occupied){
						add = true;
					}
				}
				if(add){
					for(int ii = 0; ii < nodeCenters->size(); ii ++){
						if((Vector2(n->x, n->y)-nodeCenters->at(ii)).magnitude() < 30){
							add = false;
							break;
						}
					}
				}
				if(add){
					nodeCenters->push_back(Vector2(n->x, n->y));
				}
			}
		}
	}
}

void Graph::addObstacles(){
	Vector2 center(x+((float)granularity/2.0f), y + ((float)granularity/2.0f));
	float step = granularity;
	node* n;

	for(int i = 0; i < height; i ++){
		for(int j = 0; j < width; j ++){
			n = &graph[i*width+j];
			n->nextToOccupied = false;
			n->x = center.x;
			n->y = center.y;
			n->isGoal = false;
			n->previous = NULL;
			n->flag = -1;
			n->cost = -1;

			n->occupied = false;
			if(isInObstacle(n)) n->occupied = true;
			center.x += step;
		}
		center.x = x+((float)granularity/2.0f);
		center.y += step;
	}
}

bool Graph::isInObstacle(node* n){
	vector<obstacle_t> obstacles = w->obstacles();

	for(unsigned int i = 0; i < obstacleBoundingBoxes.size(); i ++){
		if(n->x >= obstacleBoundingBoxes[i][0].x &&
		   n->y >= obstacleBoundingBoxes[i][0].y &&
		   n->x <= obstacleBoundingBoxes[i][1].x &&
		   n->y <= obstacleBoundingBoxes[i][1].y){
			   return true;
		}
	}
	return false;
}

void Graph::buildBoundingBoxes(){
	obstacleBoundingBoxes.clear();

	vector<obstacle_t> obstacles = w->obstacles();

	for(unsigned int i = 0; i < obstacles.size(); i ++){
		Vector2* corners = new Vector2[2];
		corners[0].x = obstacles[i].o_corner[0][0];
		corners[0].y = obstacles[i].o_corner[0][1];
		corners[1].x = obstacles[i].o_corner[0][0];
		corners[1].y = obstacles[i].o_corner[0][1];

		for(int j = 1; j < obstacles[i].size; j ++){
			if(corners[0].x > obstacles[i].o_corner[j][0]) corners[0].x = obstacles[i].o_corner[j][0];
			else if(corners[1].x < obstacles[i].o_corner[j][0]) corners[1].x = obstacles[i].o_corner[j][0];

			if(corners[0].y > obstacles[i].o_corner[j][1]) corners[0].y = obstacles[i].o_corner[j][1];
			else if(corners[1].y < obstacles[i].o_corner[j][1]) corners[1].y = obstacles[i].o_corner[j][1];
		}
		std::cout << "final " << corners[0].x << ", " << corners[0].y << "     " << corners[1].x << ", " << corners[1].y << std::endl;
		obstacleBoundingBoxes.push_back(corners);
	}
}